#include "pid_q15.h"
#include <stdio.h>
#include <stdbool.h>

int pid_q15_main(void)
{
    // 1. 定义PID句柄
    WHT_PID_Q15_Ctrl_t WHT_PID_Q15_Ctrl;

    // 2. 初始化PID（kp=0.5, ki=0.1, kd=0.3，输出范围[-0.8, 0.8]）
    Q15_t kp = WHT_PID_Q15.WHT_Float_To_Q15(0.5f);
    Q15_t ki = WHT_PID_Q15.WHT_Float_To_Q15(0.1f);
    Q15_t kd = WHT_PID_Q15.WHT_Float_To_Q15(0.3f);
    Q15_t out_min = WHT_PID_Q15.WHT_Float_To_Q15(-0.8f);
    Q15_t out_max = WHT_PID_Q15.WHT_Float_To_Q15(0.8f);
    WHT_PID_Q15.WHT_Init(&WHT_PID_Q15_Ctrl, kp, ki, kd, out_min, out_max);

    // 3. 配置积分限幅、微分滤波
    WHT_PID_Q15.WHT_Config_Integral_Limit(&WHT_PID_Q15_Ctrl, true, WHT_PID_Q15.WHT_Float_To_Q15(-2.0f), WHT_PID_Q15.WHT_Float_To_Q15(2.0f));
    WHT_PID_Q15.WHT_Config_Derivative_Filter(&WHT_PID_Q15_Ctrl, true, WHT_PID_Q15.WHT_Float_To_Q15(0.9f));

    // 4. 设置目标值并计算
    WHT_PID_Q15.WHT_Set_Setpoint(&WHT_PID_Q15_Ctrl, WHT_PID_Q15.WHT_Float_To_Q15(0.5f)); // 目标值0.5
    while(1)
    {
        Q15_t feedback = WHT_PID_Q15.WHT_Float_To_Q15(0.5678f); // 采集器反馈值
        Q15_t output = WHT_PID_Q15.WHT_Compute(&WHT_PID_Q15_Ctrl, feedback); // 计算输出（无自适应）
        printf("PID Out %dQ15\r\n", output);//控制执行器
        delay_ms(20);
    }
}


int pid_q15_auto_main(void)
{
    // 1. 定义PID句柄
    WHT_PID_Q15_Ctrl_t WHT_PID_Q15_Ctrl;

    // 2. 初始化PID（kp=0.5, ki=0.1, kd=0.3，输出范围[-0.8, 0.8]）
    Q15_t kp = WHT_PID_Q15.WHT_Float_To_Q15(0.5f);
    Q15_t ki = WHT_PID_Q15.WHT_Float_To_Q15(0.1f);
    Q15_t kd = WHT_PID_Q15.WHT_Float_To_Q15(0.3f);
    Q15_t out_min = WHT_PID_Q15.WHT_Float_To_Q15(-0.8f);
    Q15_t out_max = WHT_PID_Q15.WHT_Float_To_Q15(0.8f);
    WHT_PID_Q15.WHT_Init(&WHT_PID_Q15_Ctrl, kp, ki, kd, out_min, out_max);

    // 3. 配置积分限幅、微分滤波
    WHT_PID_Q15.WHT_Config_Integral_Limit(&WHT_PID_Q15_Ctrl, true, WHT_PID_Q15.WHT_Float_To_Q15(-2.0f), WHT_PID_Q15.WHT_Float_To_Q15(2.0f));
    WHT_PID_Q15.WHT_Config_Derivative_Filter(&WHT_PID_Q15_Ctrl, true, WHT_PID_Q15.WHT_Float_To_Q15(0.9f));

    // 4. 配置自适应参数
    WHT_PID_Q15.WHT_Config_Adaptive(&WHT_PID_Q15_Ctrl, true,
                                    // kp范围：0.2~0.8（Q15格式）
                                    WHT_PID_Q15.WHT_Float_To_Q15(0.2f), WHT_PID_Q15.WHT_Float_To_Q15(0.8f),
                                    // ki范围：0.05~0.2（Q15格式）
                                    WHT_PID_Q15.WHT_Float_To_Q15(0.05f), WHT_PID_Q15.WHT_Float_To_Q15(0.2f),
                                    // kd范围：0.1~0.5（Q15格式）
                                    WHT_PID_Q15.WHT_Float_To_Q15(0.1f), WHT_PID_Q15.WHT_Float_To_Q15(0.5f),
                                    // 误差阈值：大=0.125、中=0.0625、变化率小=0.03125（Q15格式）
                                    0x1000, 0x0800, 0x0400);

    // 5. 设置目标值并计算
    WHT_PID_Q15.WHT_Set_Setpoint(&WHT_PID_Q15_Ctrl, WHT_PID_Q15.WHT_Float_To_Q15(0.5f)); // 目标值0.5
    while(1)
    {
        Q15_t feedback = WHT_PID_Q15.WHT_Float_To_Q15(0.5678f); // 采集器反馈值
        Q15_t output = WHT_PID_Q15.WHT_Compute(&WHT_PID_Q15_Ctrl, feedback);// 计算输出（自动自适应调参）
        printf("PID Out %dQ15\r\n", output);//控制执行器
        delay_ms(20);
    }
}
